#include "stanley_lateral_kinematics.h"
#include "ros/ros.h"
#include "auto_msgs/localizition.h"
#include "auto_msgs/chassis.h"
#include "auto_msgs/control_cmd.h"
#include "auto_msgs/planning.h"
#include "auto_msgs/planning_point.h"
#include "common_tool/clampPid.h"
#include "common_tool/watchDog.h"
#include "stanley_pkg/stanley_debug.h"
StartStanleyNs class StanleyNode
{
    //    watchDog(int foodPerTurn=0, int hungerPerTurn=0, int hungerMin=0,\
    //int criticalPoint=50, int hungerMax=100);
private:
    watchDog localizition_dog_;
    watchDog chassis_dog_;
    watchDog planning_dog_;
    auto_msgs::planning planning_msg_;
    auto_msgs::localizition localizition_msg_;
    auto_msgs::chassis chassis_msg_;
    stanley_pkg::stanley_debug stanley_debug_msg_;
    void UpdateStanleyDebugMsg();
    void NoFood();

public:
    StanleyLateral lat_controller_;
    clampPid speed_controller_;
    StanleyNode() = delete;
    StanleyNode(double loop_rate);
    void CBLocalizition(const auto_msgs::localizition &msg);
    void CBChassis(const auto_msgs::chassis &msg);
    void CBPlanning(const auto_msgs::planning &msg);
    bool SearchRefPoint(const auto_msgs::planning &msg);
    bool CheckDog();
    void LoopProc();
    void HardBrake();
    bool LateralCtr();
    bool LongtitudeCtr();
    void UpdateStanleyLateralInput();

    //relate with  ros
    // publish apollo wrapper topic
    ros::NodeHandle nh;
    ros::Publisher control_cmd_pub_ = nh.advertise<auto_msgs::control_cmd>("control_cmd", 3);
    auto_msgs::control_cmd control_cmd_;
    ros::Publisher stanley_debug_pub_ = nh.advertise<stanley_pkg::stanley_debug>("stanley_debug", 3);

    //pub debug msg
    //ros::Publisher pure_pursuit_debug_pub =nh.advertise
    //    <pure_pursuit_pkg::pure_pursuit_debug>("pure_pursuit_debug", 3);
    //subscribe apollo wrapper topic
    ros::Rate loop_rate_;
    ros::Subscriber localizition_sub_ =
        nh.subscribe("localizition", 3, &StanleyNode::CBLocalizition, this);
    ros::Subscriber chassis_sub_ =
        nh.subscribe("chassis", 3, &StanleyNode::CBChassis, this);
    ros::Subscriber planning_sub_ =
        nh.subscribe("planning_path", 3, &StanleyNode::CBPlanning, this);
};
EndStanleyNs